#ifndef MODEL_INTERFACE_H
#define MODEL_INTERFACE_H

#include "model/model_impl/basic_data_structure.hpp"
#include "model/model_impl/differential_kinematics.hpp"
#include "model/model_impl/forward_dynamics.hpp"
#include "model/model_impl/forward_kinematics.hpp"
#include "model/model_impl/inverse_dynamics.hpp"
#include "model/model_impl/inverse_kinematics.hpp"
#include "model/model_impl/robot_model.hpp"
#include "model/model_data_structure.hpp"

using namespace Eigen;

namespace ZORAL
{
    class ZoralModel
    {
    private:
        RobotModel *rm_;
        ForwardKinematics *fk_;
        InverseKinematics *ik_;
        DifferentialKinematics *dk_;
        InverseDynamics *id_;

    public:
        ZoralModel(const std::string &robot_name, const std::string &config_path)
        {
            rm_ = new RobotModel(robot_name, config_path);
            fk_ = new ForwardKinematics(rm_);
            dk_ = new DifferentialKinematics(fk_);
            ik_ = new InverseKinematics(fk_);
            id_ = new InverseDynamics(fk_);
        }

        int getRobotDof();
        int forwardKinematics(const std::vector<double> &joint_angles, std::vector<double> &pose);
        int inverseKinematics(const std::vector<double> &pose, const IKConfig& ik_config, std::vector<double> &joint_angles);
        int inverseDynamics(const std::vector<double> &q, const std::vector<double> &qd, const std::vector<double> &qdd, std::vector<double> &torque);

        double computeArmAngle(const std::vector<double> &joint_angles);
        int computeGlobalConfig(const std::vector<double> &joint_angles);

        std::vector<std::vector<double>> getJointLimits();
    };
} // namespace ZORAL

#endif // MODEL_INTERFACE_H
